﻿
#include "WIT_Argeement.h"

#include "check.h"


bool WIT::dataSprintf(const void* pData, const size_t count, char* buffer, size_t* dataLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;//遍历数据用的指针
	size_t length = 0;
	size_t piece = 0;//某个包的长的
	
	while (pTemp < (uint8_t*)pData + count)
	{
		packetSprintf(pTemp, buffer + length, &piece);
		length += piece;
		pTemp += 11;//它一个包11个
	}

	if (dataLength != nullptr)
		*dataLength = length;
	return true;
}

bool WIT::IMUdataSprintf(const void* pData, char* buffer, size_t* strLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;

	if (
		*(pTemp + 1) != TYPE::TIME ||
		*(pTemp + 12) != TYPE::ACC ||
		*(pTemp + 23) != TYPE::GYRO ||
		*(pTemp + 34) != TYPE::ANGLE ||
		*(pTemp + 45) != TYPE::MAGNETIC ||
		*(pTemp + 56) != TYPE::QUATER
		)
		return false;

	size_t length = 0;
	size_t piece = 0;//某个包字符串的长

	if (timeSprintf(pTemp, buffer + length, &piece) != true)
		return false;
	length += piece;
	pTemp += 11;
	if (accelerationSprintf(pTemp, buffer + length, &piece) != true)
		return false;
	length += piece;
	pTemp += 11;
	if (angularSpeedSprintf(pTemp, buffer + length, &piece) != true)
		return false;
	length += piece;
	pTemp += 11;
	if (angleSprintf(pTemp, buffer + length, &piece) != true)
		return false;
	length += piece;
	pTemp += 11;
	if (magneticSprintf(pTemp, buffer + length, &piece) != true)
		return false;
	length += piece;
	pTemp += 11;
	if (quarternionSprintf(pTemp, buffer + length, &piece) != true)
		return false;
	length += piece;
	pTemp += 11;

	*(buffer + length + 1) = '\r';//最后的换行
	*(buffer + length + 2) = '\n';//最后的换行
	length += 2;

	*strLength = length;
	return true;
}

bool WIT::packetSprintf(const void* pData, char* buffer, size_t* strLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;
	size_t length = 0;
	size_t piece = 0;

	switch (*(pTemp + 1))
	{
	case TYPE::TIME://时间戳
	{
		if (timeSprintf(pTemp, buffer + length, &piece) != true)//校验不通过
			return false;
		length += piece;
		break;
	}
	case TYPE::ACC://加速度
	{
		if (accelerationSprintf(pTemp, buffer + length, &piece) != true)//校验不通过
			return false;
		length += piece;
		break;
	}
	case TYPE::GYRO://陀螺仪
	{
		if (angularSpeedSprintf(pTemp, buffer + length, &piece) != true)//校验不通过
			return false;
		length += piece;
		break;
	}
	case TYPE::ANGLE://欧拉角	
	{
		if (angleSprintf(pTemp, buffer + length, &piece) != true)//校验不通过
			return false;
		length += piece;
		break;
	}
	case TYPE::MAGNETIC://磁感应强度
	{
		if (magneticSprintf(pTemp, buffer + length, &piece) != true)//校验不通过
			return false;
		length += piece;
		break;
	}
	case TYPE::QUATER://四元数
	{
		if (quarternionSprintf(pTemp, buffer + length, &piece) != true)//校验不通过
			return false;
		length += piece;
		break;
	}
	default:
		return false;
		break;
	}

	if (strLength != nullptr)
		*strLength = length;
	return true;
}

bool WIT::getDataType(const void* pData, TYPE* theType)
{
	if (pData == nullptr || theType == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;//遍历数据用的指针
	TYPE type;
	switch (*(pTemp + 1))
	{
	case TYPE::TIME://时间戳
	{
		type = TIME;
		break;
	}
	case TYPE::ACC://加速度
	{
		type = ACC;
		break;
	}
	case TYPE::GYRO://陀螺仪
	{
		type = ACC;
		break;
	}
	case TYPE::ANGLE://欧拉角	
	{
		type = ACC;
		break;
	}
	case TYPE::MAGNETIC://磁感应强度
	{
		type = ACC;
		break;
	}
	case TYPE::QUATER://四元数
	{
		type = ACC;
		break;
	}
	default:
		return false;
		break;
	}
	return true;
}



bool WIT::getTime(const void* pData, tm* pTime, int16_t* ms)
{
	if (pData == nullptr)
		return false;

	uint8_t* typeP = (uint8_t*)pData + 1;//指向数据包类型的指针
	if (*typeP != TYPE::TIME)
		return false;

	uint8_t theSum;
	additionChecksum_8BIT(pData, 10, &theSum);
	if (theSum != *((uint8_t*)pData + 10))//校验不通过
		return false;

	uint8_t* dataP = (uint8_t*)pData + 2;//指向数据区域的指针
	
	if (pTime != nullptr)
	{
		pTime->tm_year = dataP[0] + 2000 - 1900;
		pTime->tm_mon = dataP[1] - 1;
		pTime->tm_mday = dataP[2];
		pTime->tm_hour = dataP[3];
		pTime->tm_min = dataP[4];
		pTime->tm_sec = dataP[5];
	}	

	if (ms != nullptr)
	{
		*ms = (dataP[7] << 8) | dataP[6];
	}	

	return true;
}

bool WIT::timeSprintf(const void* pData, char* buffer, size_t* strLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;//遍历数据用的指针
	size_t length = 0;
	int16_t IMU_TimeMS = 0;
	tm IMU_Time = { 0 };
	time_t currentTime = 0;
	if (getTime(pTemp, &IMU_Time, &IMU_TimeMS) != true)//校验不通过
		return false;

	currentTime = mktime(&IMU_Time);
	length = sprintf(buffer,
		"%04d-%02d-%02d %02d:%02d:%02d.%03d,",
		IMU_Time.tm_year + 1900,
		IMU_Time.tm_mon + 1,
		IMU_Time.tm_yday,
		IMU_Time.tm_hour,
		IMU_Time.tm_min,
		IMU_Time.tm_sec,
		IMU_TimeMS);
	if (strLength != nullptr)
		*strLength = length;
	
	return true;
}

bool WIT::getAcceleration(const void* pData, float* Ax, float* Ay, float* Az)
{
	if (pData == nullptr || Ax == nullptr || Ay == nullptr || Az == nullptr)
		return false;

	uint8_t* typeP = (uint8_t*)pData + 1;//指向数据包类型的指针
	if (*typeP != TYPE::ACC)
		return false;

	uint8_t theSum;
	additionChecksum_8BIT(pData, 10, &theSum);//校验
	if (theSum != *((uint8_t*)pData + 10))//校验不通过
		return false;
	uint8_t* dataP = (uint8_t*)pData + 2;//指向数据区域的指针

	*Ax = ((int16_t)((dataP[1] << 8) | dataP[0])) * (16.0 * 9.8 / 32768);//X轴加速度 单位 米每二次方秒
	*Ay = ((int16_t)((dataP[3] << 8) | dataP[2])) * (16.0 * 9.8 / 32768);//y轴加速度 单位 米每二次方秒
	*Az = ((int16_t)((dataP[5] << 8) | dataP[4])) * (16.0 * 9.8 / 32768);//z轴加速度 单位 米每二次方秒
	return true;
}

bool WIT::accelerationSprintf(const void* pData, char* buffer, size_t* strLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;//遍历数据用的指针
	size_t length = 0;
	float ax, ay, az;
	if (getAcceleration(pTemp, &ax, &ay, &az) != true)//校验不通过
		return false;

	length = sprintf(buffer, "%f,%f,%f,", ax,ay,az);
	if (strLength != nullptr)
		*strLength = length;

	return true;
}

bool WIT::getTemperature(const void* pData, float* pT)
{
	uint8_t* dataP = (uint8_t*)pData + 2;//指向数据区域的指针
	*pT = ((int16_t)((dataP[7] << 8) | dataP[6])) * 0.01;//温度 单位 摄氏度
	
	uint8_t theSum;
	additionChecksum_8BIT(pData, 10, &theSum);
	if (theSum != *((uint8_t*)pData + 10))//校验不通过
		return false;
	else
		return true;
}

bool WIT::getAngularSpeed(const void* pData, float* Wx, float* Wy, float* Wz)
{
	if (pData == nullptr || Wx == nullptr || Wy == nullptr || Wz == nullptr)
		return false;

	uint8_t* typeP = (uint8_t*)pData + 1;//指向数据包类型的指针
	if (*typeP != TYPE::GYRO)
		return false;

	uint8_t theSum;
	additionChecksum_8BIT(pData, 10, &theSum);
	if (theSum != *((uint8_t*)pData + 10))//校验不通过
		return false;

	uint8_t* dataP = (uint8_t*)pData + 2;//指向数据区域的指针
	*Wx = ((int16_t)((dataP[1] << 8) | dataP[0])) * (4000.0 / 32768);//X轴角速度 单位 度每秒
	*Wy = ((int16_t)((dataP[3] << 8) | dataP[2])) * (4000.0 / 32768);//Y轴加速度 单位 度每秒
	*Wz = ((int16_t)((dataP[5] << 8) | dataP[4])) * (4000.0 / 32768);//Z轴加速度 单位 度每秒
	return true;
}

bool WIT::angularSpeedSprintf(const void* pData, char* buffer, size_t* strLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;//遍历数据用的指针
	size_t length = 0;
	float wx, wy, wz;
	if (getAngularSpeed(pTemp, &wx, &wy, &wz) != true)//校验不通过
		return false;

	length = sprintf(buffer, "%f,%f,%f,", wx, wy, wz);
	if (strLength != nullptr)
		*strLength = length;

	return true;
}

bool WIT::getAngle(const void* pData, float* Roll, float* Pitch, float* Yaw)
{
	if (pData == nullptr || Roll == nullptr || Pitch == nullptr || Yaw == nullptr)
		return false;

	uint8_t* typeP = (uint8_t*)pData + 1;//指向数据包类型的指针
	if (*typeP != TYPE::ANGLE)
		return false;

	uint8_t theSum;
	additionChecksum_8BIT(pData, 10, &theSum);
	if (theSum != *((uint8_t*)pData + 10))//校验不通过
		return false;

	uint8_t* dataP = (uint8_t*)pData + 2;//指向数据区域的指针
	*Roll = ((int16_t)((dataP[1] << 8) | dataP[0])) * (180.0 / 32768);
	*Pitch = ((int16_t)((dataP[3] << 8) | dataP[2])) * (180.0 / 32768);
	*Yaw = ((int16_t)((dataP[5] << 8) | dataP[4])) * (180.0 / 32768);

	return true;
}

bool WIT::angleSprintf(const void* pData, char* buffer, size_t* strLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;//遍历数据用的指针
	size_t length = 0;
	float roll, pitch, yaw;
	if (getAngle(pTemp, &roll, &pitch, &yaw) != true)//校验不通过
		return false;

	length = sprintf(buffer, "%f,%f,%f,", roll, pitch, yaw);
	if (strLength != nullptr)
		*strLength = length;

	return true;
}

bool WIT::getVersion(const void* pData, uint16_t* pVersion)
{
	uint8_t* dataP = (uint8_t*)pData + 2;//指向数据区域的指针
	*pVersion = (uint16_t)((dataP[7] << 8) | dataP[6]);//版本号

	uint8_t theSum;
	additionChecksum_8BIT(pData, 10, &theSum);
	if (theSum != *((uint8_t*)pData + 10))//校验不通过
		return false;
	else
		return true;
}

bool WIT::getMagnetic(const void* pData, int16_t* Hx, int16_t* Hy, int16_t* Hz)
{
	if (pData == nullptr || Hx == nullptr || Hy == nullptr || Hz == nullptr)
		return false;

	uint8_t* typeP = (uint8_t*)pData + 1;//指向数据包类型的指针
	if (*typeP != TYPE::MAGNETIC)
		return false;

	uint8_t theSum;
	additionChecksum_8BIT(pData, 10, &theSum);
	if (theSum != *((uint8_t*)pData + 10))//校验不通过
		return false;

	uint8_t* dataP = (uint8_t*)pData + 2;//指向数据区域的指针
	*Hx = ((int16_t)((dataP[1] << 8) | dataP[0]));
	*Hy = ((int16_t)((dataP[3] << 8) | dataP[2]));
	*Hz = ((int16_t)((dataP[5] << 8) | dataP[4]));
	return true;
}

bool WIT::magneticSprintf(const void* pData, char* buffer, size_t* strLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;//遍历数据用的指针
	size_t length = 0;
	int16_t hx, hy, hz;
	if (getMagnetic(pTemp, &hx, &hy, &hz) != true)//校验不通过
		return false;

	length = sprintf(buffer, "%d,%d,%d,", hx, hy, hz);
	if (strLength != nullptr)
		*strLength = length;

	return true;
}

bool WIT::getQuarternion(const void* pData, float* Q0, float* Q1, float* Q2, float* Q3)
{
	if (pData == nullptr || Q0 == nullptr || Q1 == nullptr || Q2 == nullptr || Q3 == nullptr)
		return false;

	uint8_t* typeP = (uint8_t*)pData + 1;//指向数据包类型的指针
	if (*typeP != TYPE::QUATER)
		return false;

	uint8_t theSum;
	additionChecksum_8BIT(pData, 10, &theSum);
	if (theSum != *((uint8_t*)pData + 10))//校验不通过
		return false;

	uint8_t* dataP = (uint8_t*)pData + 2;//指向数据区域的指针
	*Q0 = ((int16_t)((dataP[1] << 8) | dataP[0])) / 32768.0;
	*Q1 = ((int16_t)((dataP[3] << 8) | dataP[2])) / 32768.0;
	*Q2 = ((int16_t)((dataP[5] << 8) | dataP[4])) / 32768.0;
	*Q3 = ((int16_t)((dataP[7] << 8) | dataP[6])) / 32768.0;
		
	return true;
}

bool WIT::quarternionSprintf(const void* pData, char* buffer, size_t* strLength)
{
	if (pData == nullptr || buffer == nullptr)
		return false;

	uint8_t* pTemp = (uint8_t*)pData;//遍历数据用的指针
	size_t length = 0;
	float q0, q1, q2, q3;
	if (getQuarternion(pTemp, &q0, &q1, &q2, &q3) != true)//校验不通过
		return false;

	length = sprintf(buffer, "%f,%f,%f,%f,", q0, q1, q2, q3);
	if (strLength != nullptr)
		*strLength = length;

	return true;
}
